#include <ros/ros.h>
#include <iostream>
#include "./../../kinematics/Kine.h"
#include <birl_manipulator/ForwardKine.h>

bool handle_function( birl_manipulator::ForwardKine::Request &req,
                      birl_manipulator::ForwardKine::Response &res)
{
    // 机器人基础连杆长度设置
    static double Robot_Link_Len[6];
    Robot_Link_Len[0] = 0.20400;
    Robot_Link_Len[1] = 0.13670;
    Robot_Link_Len[2] = 0.29320;
    Robot_Link_Len[3] = 0.29320;
    Robot_Link_Len[4] = 0.13670;
    Robot_Link_Len[5] = 0.20400;

    static double decartes_position_value[6];   //(xyzwpr) unit:(mm,degree)
    static double current_joint_value[5];   //unit:degree

    // Note: 由于机器人是从机械零点开始运动 因此第2、3T关节需要增加90度，此为相对于运动学模型绝对零点的角度值
    // 机器人运动学模型绝对零点参见用户手册附录最后一页
    current_joint_value[0] = req.current_joint_state[0];
    current_joint_value[1] = req.current_joint_state[1]+90; 
    current_joint_value[2] = req.current_joint_state[2]+90;
    current_joint_value[3] = req.current_joint_state[3];
    current_joint_value[4] = req.current_joint_state[4];

    static Kine_IR_FiveDoF manipulator;
    manipulator.Set_Length(Robot_Link_Len);
    // 调用正运动学模型
    if(!manipulator.FKine(current_joint_value,decartes_position_value))
    {
        ROS_INFO_STREAM("FKine Success");
        res.ifGetSolve = true;
    }
    else {
        res.ifGetSolve = false;
    }
    res.descartes_pos_state.clear();
    for(int i =0;i<6;i++)
    {
        res.descartes_pos_state.push_back(decartes_position_value[i]);
    }
    return true;
}

int main(int argc,char* argv[])
{
    ros::init(argc,argv,"manipulator_fk_server");
    ros::NodeHandle node_handle;
    ros::ServiceServer server = node_handle.advertiseService("ForwardKine",handle_function);
    ROS_INFO_STREAM("fk_server is Setted");
    ros::spin();
    return 0;

}